package org.example.util;

/**
 * @ClassName RRT
 * @Description TODO
 * @Author hongjun
 * @Date 2024/5/10 下午11:31
 * @Version 1.0
 */
import java.util.*;

public class RRT {

    public static class Node {
        public double x, y;
        public Node parent;

        public Node(double x, double y, Node parent) {
            this.x = x;
            this.y = y;
            this.parent = parent;
        }
    }

    public static List<Node> rrtSearch(int[][] map, double startX, double startY, double endX, double endY) {
        Random rand = new Random();
        final double DELTA_Q = 0.1; // 步长
        final double SEARCH_RADIUS = 1.0; // 搜索半径
        final int MAX_ITERATIONS = 10000; // 最大迭代次数
        final double GOAL_THRESHOLD = 0.5; // 达到目标的阈值距离

        List<Node> nodes = new ArrayList<>();
        List<Node> path = new ArrayList<>();
        nodes.add(new Node(startX, startY, null)); // 添加起点

        for (int i = 0; i < MAX_ITERATIONS; i++) {
            double randX = rand.nextDouble() * (map.length - 1);
            double randY = rand.nextDouble() * (map[0].length - 1);
            Node nearest = findNearest(nodes, randX, randY);
            if (nearest == null) continue;

            double dx = randX - nearest.x;
            double dy = randY - nearest.y;
            double distance = Math.sqrt(dx * dx + dy * dy);
            if (distance == 0) continue;

            double stepSize = Math.min(DELTA_Q / distance, 1.0);
            double newX = nearest.x + stepSize * dx;
            double newY = nearest.y + stepSize * dy;

            // 检查碰撞
            if (map[(int) newX][(int) newY] == 1) continue;

            Node newNode = new Node(newX, newY, nearest);
            nodes.add(newNode);

            // 检查是否达到目标
            if (distanceToTarget(newNode, endX, endY) <= GOAL_THRESHOLD) {
                generatePath(newNode, path);
                return path;
            }
        }
        return null; // 没有找到路径
    }

    private static Node findNearest(List<Node> nodes, double x, double y) {
        Node nearest = null;
        double minDist = Double.MAX_VALUE;
        for (Node node : nodes) {
            double dist = distance(node.x, node.y, x, y);
            if (dist < minDist) {
                minDist = dist;
                nearest = node;
            }
        }
        return nearest;
    }

    private static double distance(double x1, double y1, double x2, double y2) {
        return Math.sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
    }

    private static void generatePath(Node node, List<Node> path) {
        while (node != null) {
            path.add(0, node); // 在列表前面添加以保持路径顺序
            node = node.parent;
        }
    }

    private static double distanceToTarget(Node node, double targetX, double targetY) {
        return distance(node.x, node.y, targetX, targetY);
    }
}